#include "global.h"

// 定义低通滤波器数组，用于对遥控数据进行滤波处理
static _st_LowPassFilter lpf[4];

/**
 * 分析和处理遥控数据
 *
 * 1. 将ADC值归一化到1000到2000之间的范围。
 * 2. 对归一化后的数据进行低通滤波处理。
 * 3. 加上偏移量，并进行限幅处理（限制在1000到2000之间）。
 */
void txRemote_generateData(void)
{
    // 归一化处理：将ADC值转换为1000到2000之间的值
    int16_t thr_normalized = 1000 + (int16_t)((adcValue[1] / 4095.0f) * 1000.0f);
    int16_t pitch_normalized = 1000 + (int16_t)((adcValue[2] / 4095.0f) * 1000.0f);
    int16_t yaw_normalized = 1000 + (int16_t)((adcValue[0] / 4095.0f) * 1000.0f);
    int16_t roll_normalized = 1000 + (int16_t)((adcValue[3] / 4095.0f) * 1000.0f);

    // 滤波处理：先滤波再加偏移
    lowPass_update(&lpf[0], (float)thr_normalized); // 一维低通滤波
    txRemote.thr = (int16_t)lpf[0].x + txRemoteOffset.thr;
    LIMIT_I16(&txRemote.thr, 1000, 2000); // 限幅处理

    lowPass_update(&lpf[1], (float)pitch_normalized); // 一维低通滤波
    txRemote.pitch = (int16_t)lpf[1].x + txRemoteOffset.pitch;
    LIMIT_I16(&txRemote.pitch, 1000, 2000); // 限幅处理

    lowPass_update(&lpf[2], (float)yaw_normalized); // 一维低通滤波
    txRemote.yaw = (int16_t)lpf[2].x + txRemoteOffset.yaw;
    LIMIT_I16(&txRemote.yaw, 1000, 2000); // 限幅处理

    lowPass_update(&lpf[3], (float)roll_normalized); // 一维低通滤波
    txRemote.roll = (int16_t)lpf[3].x + txRemoteOffset.roll;
    LIMIT_I16(&txRemote.roll, 1000, 2000); // 限幅处理
}

/**
 * 发送遥控数据
 *
 * 功能：
 * 1. 使用静态变量控制发送步骤。
 * 2. 在步骤0中生成遥控数据并通过NRF24L01模块发送。
 * 3. 在步骤1中等待接收端确认数据，并处理丢失计数。
 * 4. 每秒更新丢包数量并重置计数器。
 */
void txRemote_send(void)
{
    static uint8_t step = 0;                 // 控制发送步骤
    static uint16_t timeoutCounter = 0;      // 超时计数器
    static uint16_t txRemoteLostCounter = 0; // 数据丢失计数器
    uint8_t getAckStatus;                    // 接收端确认状态
    uint8_t ackPayload[32];                  // 确认数据缓冲区
    uint8_t ackPayloadLen = 0;               // 确认数据长度

    switch (step)
    {
    case 0:
        // 发送遥控数据到接收端
        txRemote_generateData(); // 生成遥控数据
        nrf24l01_sendData((uint8_t *)&txRemote, sizeof(txRemote));
        step = 1; // 切换到等待确认步骤
        break;

    case 1:
        // 获取接收端的确认数据
        getAckStatus = nrf24l01_getAck(ackPayload, &ackPayloadLen);
        if (getAckStatus == SUCCESS && ackPayloadLen == sizeof(rxRemote))
        {
            memcpy(&rxRemote, ackPayload, sizeof(rxRemote)); // 更新接收端数据
        }
        else
        {
            // 如果未收到确认数据，则增加丢失计数
            txRemoteLostCounter++;
        }
        step = 0; // 切换回发送步骤
        break;

    default:
        // 处理未预期的状态
        step = 0;
        break;
    }

    // 处理超时计数器
    if (++timeoutCounter >= COUNTER1S)
    {
        timeoutCounter = 0;                 // 重置计数器
        txRemoteLost = txRemoteLostCounter; // 更新丢包数量
        if (txRemoteLost > 99)              // 一秒最多丢包100个，超过99则认为无人机掉线
        {
            rxRemote.state = 0;    // 设置接收端状态为0
            rxRemote.roll = 0.0f;  // 设置接收端roll为0
            rxRemote.pitch = 0.0f; // 设置接收端pitch为0
            rxRemote.yaw = 0.0f;   // 设置接收端yaw为0
        }
        txRemoteLostCounter = 0; // 重置丢失计数器
    }
}

/**
 * 初始化低通滤波器
 *
 * 功能：初始化4个低通滤波器实例，用于滤波遥控数据。
 */
void txRemote_fliterInit(void)
{
    for (uint8_t i = 0; i < 4; i++)
    {
        lowPass_init(&lpf[i], 0.15f); // 初始化低通滤波器
    }
}

/**
 * 从EEPROM加载偏移量
 *
 * 功能：从AT24C02 EEPROM读取roll和pitch的偏移量。
 */
void txRemote_loadOffset(void)
{
    // 从AT24C02 EEPROM读取roll和pitch的偏移量
    at24c02_readPage(AT24C02ADDR_ROLL_OFFSET, (uint8_t *)&txRemoteOffset.roll, 2);
    at24c02_readPage(AT24C02ADDR_PITCH_OFFSET, (uint8_t *)&txRemoteOffset.pitch, 2);
}
